package org.hs.phd.odi.tracking.tracker;

import java.awt.Rectangle;
import java.awt.geom.Point2D;

import org.hs.phd.common.improc.traversal.RGBImagePixelProcessor;
import org.hs.phd.odi.tracking.tracker.kernel.Kernel;

import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

public class TargetCandidate {
	
	class MeanshiftCalculatingPixelProcessor implements RGBImagePixelProcessor{
	
		
		private static final float NEAR_ZERO = 0.0000001F;
		private float weightSum;
		private float imageGridCenterX;
		private float imageGridCenterY;
		private Kernel kernel;
		private float weightedSumX;
		private float weightedSumY;
		private float normalizationKernelRadiusX;
		private float normalizationKernelRadiusY;
		private Rectangle rectangle;
		
		public MeanshiftCalculatingPixelProcessor(){
			weightSum = 0;
			rectangle = trackWindow.getAsPolygon().getBounds();
			normalizationKernelRadiusX = rectangle.width/2.0f;
			normalizationKernelRadiusY = rectangle.height/2.0f;
			computeGridCenter();
			kernel = targetHistogram.getKernel();
			weightedSumX = 0;
		    weightedSumY = 0;
		}


		private void computeGridCenter() {
			java.awt.geom.Point2D.Float gridCenter = getGridCenter();
			imageGridCenterX = gridCenter.x  - rectangle.x;
			imageGridCenterY = gridCenter.y - rectangle.y ;
		}

		@Override
		public void processPixel(int x, int y, int r, int g, int b){
			int histogramIndex = targetHistogram.getHistogramIndex(r,g,b);
			float qu = targetHistogram.getData()[histogramIndex];
			float pu = candidateHistogram.getData()[histogramIndex];
			float initialWeight = (pu < NEAR_ZERO) ? 0 : (float)Math.sqrt(qu/pu);
			
			
			float distX = (x - rectangle.x - imageGridCenterX)/normalizationKernelRadiusX; 
			float distY = (y - rectangle.y - imageGridCenterY)/normalizationKernelRadiusY; 
			
			float kernelDerivative = kernel.derivative(distX/normalizationKernelRadiusX, distY/normalizationKernelRadiusY);
			float weight = kernelDerivative*initialWeight;
			weightSum += weight;
			
		    weightedSumX += weight*distX;
		    weightedSumY += weight*distY;
			
		}

		public java.awt.geom.Point2D.Float getCalculatedNextMeanshiftPosition() {
		    float meanshiftX = (weightSum < NEAR_ZERO) ? 0 : weightedSumX/weightSum;
		    float meanshiftY = (weightSum < NEAR_ZERO) ? 0 : weightedSumY/weightSum;

		    //note we add to trackWindow.getCenter().x rather than gridcenterX, because the result returned from this method
		    //should be compatible with TrackWindow.getCenter() method, which is not gridcenter
		    return new Point2D.Float( trackWindow.getCenter().x + meanshiftX, trackWindow.getCenter().y + meanshiftY );
		}
		
	};

	

	private TrackWindow trackWindow;
	private Histogram targetHistogram;
	private SimilarityMeasure similarityMeasure;
	private IplImage frame;

	private float targetSimilarity;
	private Histogram candidateHistogram;
	
	public TargetCandidate( final opencv_core.IplImage frame, TrackWindow trackWindow, Histogram targetHistogram, SimilarityMeasure similarityMeasure) {
		this.frame = frame;
		this.trackWindow = trackWindow;
		this.targetHistogram = targetHistogram;
		this.similarityMeasure = similarityMeasure;

		candidateHistogram = targetHistogram.getCopy();
		candidateHistogram.reset();
		
		recalculateSimilarity();
	}

	private void recalculateSimilarity() {
		trackWindow.calculateHistogram(frame, candidateHistogram);
		targetSimilarity = similarityMeasure.calculateSimilarity(targetHistogram, candidateHistogram);
	}

	public TargetCandidate getMovedCopy(Point2D.Float newPosition) {
		TrackWindow trackWindowCopy = trackWindow.getCopy();
		trackWindowCopy.moveCenterToPosition(newPosition);

		return new TargetCandidate(frame, trackWindowCopy, targetHistogram, similarityMeasure);
	}


	public void moveTo(Point2D.Float newPosition) {
		trackWindow.moveCenterToPosition(newPosition);
		recalculateSimilarity();
	}

	public boolean isCloseEnough(TargetCandidate other) {
		double distance = trackWindow.distanceTo(other.trackWindow);
		return distance < 0.01F;
	}

	public float getTargetSimilarity() {
		return targetSimilarity;
	}

	public void copyTo(TargetCandidate other) {
		other.trackWindow = trackWindow.getCopy();
		other.targetHistogram = targetHistogram;
		other.similarityMeasure = similarityMeasure;
		other.targetSimilarity = targetSimilarity;
		other.candidateHistogram = candidateHistogram.getCopy();
		other.frame = frame;
	}

	public void moveTowards(TargetCandidate towards, float distanceMultiplier) {
		java.awt.geom.Point2D.Float thisPoint = trackWindow.getCenter();
		java.awt.geom.Point2D.Float towardsPoint = towards.getTrackWindow().getCenter();
		
		float distanceX = towardsPoint.x - thisPoint.x;
		float distanceY = towardsPoint.y - thisPoint.y;
		
		Point2D.Float newPosition = new Point2D.Float(thisPoint.x + distanceMultiplier*distanceX,
				thisPoint.y + distanceMultiplier*distanceY);
		
		trackWindow.moveCenterToPosition(newPosition);
		
		recalculateSimilarity();
	}


	public TrackWindow getTrackWindow() {
		return trackWindow;
	}
	

	public Point2D.Float calculateNextMeanshiftPosition() {
		
		MeanshiftCalculatingPixelProcessor processor = new MeanshiftCalculatingPixelProcessor();
		trackWindow.traversePixels( frame, processor);
		return processor.getCalculatedNextMeanshiftPosition();
	}	
	
	/**
	 * Example: Consider a 3x3 image, we have indices 0, 1 and 2
	 *    If we use regular centerX = 3/2 = 1.5, 
	 *    then this center is (1.5-0) = 1.5 pixels away from left most pixel,
	 *    but (2 - 1.5) = 0.5 pixels away from the right most pixel
	 *    But if this is centerX, then by definition it has to be the same distance away from both ends!
	 *    if we calculate center as width/2.0 - 0.5, we get same distance to both ends
	 */
	public Point2D.Float getGridCenter() {
		Rectangle rectangle = trackWindow.getAsPolygon().getBounds();
		return TrackerUtils.getGridCenter(rectangle);
	}


	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + ((candidateHistogram == null) ? 0 : candidateHistogram.hashCode());
		result = prime * result + ((frame == null) ? 0 : frame.hashCode());
		result = prime * result + ((similarityMeasure == null) ? 0 : similarityMeasure.getClass().hashCode());
		result = prime * result + ((targetHistogram == null) ? 0 : targetHistogram.hashCode());
		result = prime * result + Float.floatToIntBits(targetSimilarity);
		result = prime * result + ((trackWindow == null) ? 0 : trackWindow.hashCode());
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		TargetCandidate other = (TargetCandidate) obj;
		if (candidateHistogram == null) {
			if (other.candidateHistogram != null)
				return false;
		} else if (!candidateHistogram.equals(other.candidateHistogram))
			return false;
		if (frame != other.frame) 
				return false;
		if (similarityMeasure == null) {
			if (other.similarityMeasure != null)
				return false;
		} else if (similarityMeasure.getClass() != other.similarityMeasure.getClass())
			return false;
		if (targetHistogram == null) {
			if (other.targetHistogram != null)
				return false;
		} else if (!targetHistogram.equals(other.targetHistogram))
			return false;
		if (Float.floatToIntBits(targetSimilarity) != Float.floatToIntBits(other.targetSimilarity))
			return false;
		if (trackWindow == null) {
			if (other.trackWindow != null)
				return false;
		} else if (!trackWindow.equals(other.trackWindow))
			return false;
		return true;
	}

	public Histogram getCandidateHistogram() {
		return candidateHistogram;
	}

	public Histogram getTargetHistogram() {
		return targetHistogram;
	}

	public IplImage getFrame() {
		return frame;
	}

}
